Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Optimizations for the navfn planner in large map scenarios #4244

Open
wants to merge 1 commit into
base: humble
Choose a base branch
from
Open

Optimizations for the navfn planner in large map scenarios #4244

wants to merge 1 commit into from

Conversation

XinyuKhan
Copy link

Basic Info

Info Please fill out this column
Ticket(s) this addresses (add tickets here #4243)
Primary OS tested on Ubuntu
Robotic platform tested on gazebo simulation of Tally
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

  1. Avoid repetitive memory allocation and deallocation;
  2. Avoid unnecessary map traversal;
  3. Use std::memset or std::fill instead of a for loop.

before change:

[component_container_isolated-6] [INFO] [1712419510.305826643] [bt_navigator]: Begin navigating from current location (1.53, -0.61) to (-2.33, 0.19)
[component_container_isolated-6] [WARN] [1712419510.396399095] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.0735 Hz
[component_container_isolated-6] [INFO] [1712419510.416545751] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-6] [WARN] [1712419511.504568379] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.7862 Hz
[component_container_isolated-6] [INFO] [1712419511.516851913] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419512.612415462] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.6330 Hz
[component_container_isolated-6] [INFO] [1712419512.666831014] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419513.721952562] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.7125 Hz
[component_container_isolated-6] [INFO] [1712419513.766835490] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419514.831260982] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.7334 Hz
[component_container_isolated-6] [INFO] [1712419514.866702982] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419515.942198139] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.6323 Hz
[component_container_isolated-6] [INFO] [1712419515.966687909] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419517.057747510] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.9449 Hz
[component_container_isolated-6] [INFO] [1712419517.116698077] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419518.239859170] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.5029 Hz
[component_container_isolated-6] [INFO] [1712419518.266711146] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419519.358471549] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.8443 Hz
[component_container_isolated-6] [INFO] [1712419519.416706815] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419520.480007637] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.6521 Hz
[component_container_isolated-6] [INFO] [1712419520.516705441] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419521.606562860] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.9695 Hz
[component_container_isolated-6] [INFO] [1712419521.666690680] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419522.740269989] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.6144 Hz
[component_container_isolated-6] [INFO] [1712419522.766692174] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419523.852446068] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.5828 Hz
[component_container_isolated-6] [INFO] [1712419523.866674268] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419524.967602968] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.9646 Hz
[component_container_isolated-6] [INFO] [1712419525.016711211] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419526.122193143] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.9463 Hz
[component_container_isolated-6] [INFO] [1712419526.166718255] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419527.255724271] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.1580 Hz
[component_container_isolated-6] [INFO] [1712419527.266730648] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419528.345932597] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.5242 Hz
[component_container_isolated-6] [INFO] [1712419528.366703842] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419529.469556256] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.7167 Hz
[component_container_isolated-6] [INFO] [1712419529.516679885] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419530.583000957] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.5234 Hz
[component_container_isolated-6] [INFO] [1712419530.616703278] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419531.020662321] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712419531.055845047] [bt_navigator]: Goal succeeded

after change

[component_container_isolated-6] [INFO] [1712419809.126727115] [bt_navigator]: Begin navigating from current location (-2.11, -0.15) to (1.75, -0.65)
[component_container_isolated-6] [WARN] [1712419809.178561268] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.4695 Hz
[component_container_isolated-6] [INFO] [1712419809.197766687] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-6] [WARN] [1712419810.260866628] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.7728 Hz
[component_container_isolated-6] [INFO] [1712419810.298255490] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419811.348251044] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419812.448262406] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419813.498260756] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419814.548278205] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419815.647936526] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419816.697943675] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419817.764847469] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 17.2670 Hz
[component_container_isolated-6] [INFO] [1712419817.797905056] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419818.847910680] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419819.947894103] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419820.997894306] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419822.053832724] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 17.6348 Hz
[component_container_isolated-6] [INFO] [1712419822.097913229] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419823.147923333] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419824.197926737] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419825.297902760] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419826.347922785] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419827.447926615] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419827.652004457] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712419827.686569069] [bt_navigator]: Goal succeeded

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

  • Check that any new parameters added are updated in navigation.ros.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

@mergify Mergify
Copy link
Contributor

mergify bot commented Apr 6, 2024

@XinyuKhan, all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @humble, but it must be in main
to have these changes reflected into new distributions.

SteveMacenski
SteveMacenski reviewed Apr 6, 2024
View reviewed changes
nav2_navfn_planner/src/navfn.cpp
auto new_ns = nx * ny;

if (new_ns == ns) {
return;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we not still set the costarr and pending to 0?

@SteveMacenski
Copy link
Member

SteveMacenski commented Apr 6, 2024 •
edited
Loading

There are a few changes:

  • setNavArr
  • setupNavFn
  • Added 2 debug preprocessor blocks

Can you highlight the runtime impact of each of these changes? Also you mention that this speeds up the planner in general, please provide metrics for before and after on your benchmark to understand the improvements made

This PR also needs to be rebased to main, I can backport to Humble, but all new work must be in main to be reflected in all new distributions.

@XinyuKhan
Copy link
Author

There are a few changes:

  • setNavArr
  • setupNavFn
  • Added 2 debug preprocessor blocks

Can you highlight the runtime impact of each of these changes? Also you mention that this speeds up the planner in general, please provide metrics for before and after on your benchmark to understand the improvements made

This PR also needs to be rebased to main, I can backport to Humble, but all new work must be in main to be reflected in all new distributions.

That's a good advice, but I need some time to do some tests.

@XinyuKhan
Copy link
Author

There are a few changes:

  • setNavArr
  • setupNavFn
  • Added 2 debug preprocessor blocks

Can you highlight the runtime impact of each of these changes? Also you mention that this speeds up the planner in general, please provide metrics for before and after on your benchmark to understand the improvements made
This PR also needs to be rebased to main, I can backport to Humble, but all new work must be in main to be reflected in all new distributions.

That's a good advice, but I need some time to do some tests.

Here is the test results:

origin:

heatmap:

heatmap_origin

log:

[component_container_isolated-6] [WARN] [1712502780.775772104] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.8489 Hz
[component_container_isolated-6] [INFO] [1712502780.830958993] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502781.949104795] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.6624 Hz
[component_container_isolated-6] [INFO] [1712502781.980901983] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502783.140534794] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.1982 Hz
[component_container_isolated-6] [INFO] [1712502783.180881660] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502784.397907490] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 4.3780 Hz
[component_container_isolated-6] [INFO] [1712502784.430920589] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502785.558296007] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.1866 Hz
[component_container_isolated-6] [INFO] [1712502785.580922313] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502786.719285330] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.1501 Hz
[component_container_isolated-6] [INFO] [1712502786.780879788] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502787.890919749] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.0530 Hz
[component_container_isolated-6] [INFO] [1712502787.930884112] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502789.057849226] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.2166 Hz
[component_container_isolated-6] [INFO] [1712502789.080970036] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502790.220343244] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.0901 Hz
[component_container_isolated-6] [INFO] [1712502790.280872493] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502791.410952318] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.1771 Hz
[component_container_isolated-6] [INFO] [1712502791.430894820] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502792.589177545] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.6651 Hz
[component_container_isolated-6] [INFO] [1712502792.630973065] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502793.757691487] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.2100 Hz
[component_container_isolated-6] [INFO] [1712502793.780912986] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502794.929971355] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.6340 Hz
[component_container_isolated-6] [INFO] [1712502794.980883824] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502796.100262904] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.0829 Hz
[component_container_isolated-6] [INFO] [1712502796.130893250] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712502796.434693464] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712502796.468727916] [bt_navigator]: Goal succeeded

No debug info:

heatmap:

heatmap_no_debug_info

log:

[component_container_isolated-6] [WARN] [1712503095.496224853] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.7427 Hz
[component_container_isolated-6] [INFO] [1712503095.528353103] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503096.653798053] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.3624 Hz
[component_container_isolated-6] [INFO] [1712503096.678381167] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503097.811725154] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.4561 Hz
[component_container_isolated-6] [INFO] [1712503097.828368231] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503098.982560715] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.9256 Hz
[component_container_isolated-6] [INFO] [1712503099.028365828] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503100.145601639] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.2490 Hz
[component_container_isolated-6] [INFO] [1712503100.178347592] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503101.300538826] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.5454 Hz
[component_container_isolated-6] [INFO] [1712503101.328350956] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503102.489682982] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.1987 Hz
[component_container_isolated-6] [INFO] [1712503102.528344975] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503103.620119935] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-6] [WARN] [1712503103.750116318] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-6] [WARN] [1712503103.781381155] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 3.7982 Hz
[component_container_isolated-6] [INFO] [1712503103.828415911] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503104.943285845] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.3995 Hz
[component_container_isolated-6] [INFO] [1712503104.978332910] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503106.099112790] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.6074 Hz
[component_container_isolated-6] [INFO] [1712503106.128336811] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503107.261239182] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.5013 Hz
[component_container_isolated-6] [INFO] [1712503107.278327611] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503108.417016526] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.7461 Hz
[component_container_isolated-6] [INFO] [1712503108.428321611] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503109.574774285] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.7995 Hz
[component_container_isolated-6] [INFO] [1712503109.628329590] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712503110.432371063] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712503110.467337745] [bt_navigator]: Goal succeeded

fill and memset:

heatmap:

heatmap_fill_and_memset

log:

[component_container_isolated-6] [WARN] [1712501965.803225529] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 14.2463 Hz
[component_container_isolated-6] [INFO] [1712501965.853893996] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501966.906186567] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 13.6980 Hz
[component_container_isolated-6] [INFO] [1712501966.953889824] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501968.002165682] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 14.5333 Hz
[component_container_isolated-6] [INFO] [1712501968.053892152] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501969.113792948] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.0388 Hz
[component_container_isolated-6] [INFO] [1712501969.153938581] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501970.220819999] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.9332 Hz
[component_container_isolated-6] [INFO] [1712501970.253929709] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501971.326814685] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.9537 Hz
[component_container_isolated-6] [INFO] [1712501971.353821174] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501972.436967346] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.9931 Hz
[component_container_isolated-6] [INFO] [1712501972.453771302] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501973.536102459] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 13.6637 Hz
[component_container_isolated-6] [INFO] [1712501973.553775725] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501974.630920948] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 14.7525 Hz
[component_container_isolated-6] [INFO] [1712501974.653801734] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501975.723641930] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 14.2033 Hz
[component_container_isolated-6] [INFO] [1712501975.753767242] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501976.827910055] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 13.3183 Hz
[component_container_isolated-6] [INFO] [1712501976.853767451] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501977.931593677] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.7291 Hz
[component_container_isolated-6] [INFO] [1712501977.953772660] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501979.033853595] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.3315 Hz
[component_container_isolated-6] [INFO] [1712501979.053891269] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712501980.144254742] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.3276 Hz
[component_container_isolated-6] [INFO] [1712501980.203763265] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712501980.808716824] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712501980.842564951] [bt_navigator]: Goal succeeded

memory allocate

heatmap:

heatmap_memory

log:

[component_container_isolated-6] [WARN] [1712502371.066840199] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.0723 Hz
[component_container_isolated-6] [INFO] [1712502371.118538636] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502372.214845043] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.4747 Hz
[component_container_isolated-6] [INFO] [1712502372.268566786] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502373.352589861] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.6201 Hz
[component_container_isolated-6] [INFO] [1712502373.368596103] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502374.488446346] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.9457 Hz
[component_container_isolated-6] [INFO] [1712502374.518525352] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502375.640796805] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.0873 Hz
[component_container_isolated-6] [INFO] [1712502375.668563003] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502376.780773821] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.7740 Hz
[component_container_isolated-6] [INFO] [1712502376.818563354] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502377.920977437] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.7421 Hz
[component_container_isolated-6] [INFO] [1712502377.968582205] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502379.063719262] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.5804 Hz
[component_container_isolated-6] [INFO] [1712502379.118528055] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502380.195911950] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.1943 Hz
[component_container_isolated-6] [INFO] [1712502380.218531430] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502381.330464447] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.7938 Hz
[component_container_isolated-6] [INFO] [1712502381.368553181] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502382.483840904] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.8642 Hz
[component_container_isolated-6] [INFO] [1712502382.518545248] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502383.628779657] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 8.1878 Hz
[component_container_isolated-6] [INFO] [1712502383.668550022] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502384.766645580] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.0869 Hz
[component_container_isolated-6] [INFO] [1712502384.818557396] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712502385.901603292] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.5215 Hz
[component_container_isolated-6] [INFO] [1712502385.918555762] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712502386.022658595] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712502386.056236734] [bt_navigator]: Goal succeeded

all

heatmap:

heatmap_all

log:

[component_container_isolated-6] [WARN] [1712503481.359536347] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 16.8673 Hz
[component_container_isolated-6] [INFO] [1712503481.370288027] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-6] [WARN] [1712503482.433571514] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.7792 Hz
[component_container_isolated-6] [INFO] [1712503482.470714293] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503483.522442792] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 16.0881 Hz
[component_container_isolated-6] [INFO] [1712503483.570695555] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503484.600309788] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 16.7615 Hz
[component_container_isolated-6] [INFO] [1712503484.620720841] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503485.694226905] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 15.7225 Hz
[component_container_isolated-6] [INFO] [1712503485.720713503] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503486.773826208] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.7537 Hz
[component_container_isolated-6] [INFO] [1712503486.820702908] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503487.856192817] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.0292 Hz
[component_container_isolated-6] [INFO] [1712503487.870731079] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503488.942155541] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 16.2096 Hz
[component_container_isolated-6] [INFO] [1712503488.970765263] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503490.024566550] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.6103 Hz
[component_container_isolated-6] [INFO] [1712503490.070692747] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712503491.120706218] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503492.177805319] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 17.4791 Hz
[component_container_isolated-6] [INFO] [1712503492.220716102] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503493.272749782] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 13.9141 Hz
[component_container_isolated-6] [INFO] [1712503493.320714886] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712503494.370724457] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503495.423707508] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.8114 Hz
[component_container_isolated-6] [INFO] [1712503495.470684606] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503496.502086950] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.2704 Hz
[component_container_isolated-6] [INFO] [1712503496.520707989] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712503497.620681976] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503498.657009331] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 17.6948 Hz
[component_container_isolated-6] [INFO] [1712503498.670698759] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712503499.761913228] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.2771 Hz
[component_container_isolated-6] [INFO] [1712503499.820723750] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712503500.870491599] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712503500.924946812] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712503500.959814084] [bt_navigator]: Goal succeeded

@SteveMacenski
Copy link
Member

SteveMacenski commented Apr 8, 2024 •
edited
Loading

Please provide with real hard metrics (time in ms) after release optimizations are applied - that is not sufficient to understand the practical difference to each change.

@XinyuKhan
Copy link
Author

Please provide with real hard metrics (time in ms) after release optimizations are applied - that is not sufficient to understand the practical difference to each change.

ok

@XinyuKhan
Copy link
Author

Please provide with real hard metrics (time in ms) after release optimizations are applied - that is not sufficient to understand the practical difference to each change.

According to my understanding, can I compare the test results one by one after applying each change with those without changes?

@SteveMacenski
Copy link
Member

Yes, you can also test using test/planner_benchmark to get some good metrics of hundreds of runs so statistical noise can be smoothed out a bit. There's a couple of these changes that I'm not convinced actually have a measurable impact.

Also, please respond to one of my comments - I think you bypass a 0 reset that is probably necessary

@XinyuKhan
Copy link
Author

Yes, you can also test using test/planner_benchmark to get some good metrics of hundreds of runs so statistical noise can be smoothed out a bit. There's a couple of these changes that I'm not convinced actually have a measurable impact.

Also, please respond to one of my comments - I think you bypass a 0 reset that is probably necessary

Thanks for your suggestion, I will do it soon. 0 Reset bypass issue I will reconsider and give results

@XinyuKhan
Copy link
Author

Yes, you can also test using test/planner_benchmark to get some good metrics of hundreds of runs so statistical noise can be smoothed out a bit. There's a couple of these changes that I'm not convinced actually have a measurable impact.

Also, please respond to one of my comments - I think you bypass a 0 reset that is probably necessary

Maybe I need to redo the tests on the main branch, so I need some time to re-configurate my building environment.

@SteveMacenski
Copy link
Member

Ok! Its important if it accelerates things we have an appreciation for what/how much 🙂

@XinyuKhan
Copy link
Author

XinyuKhan commented Apr 10, 2024 •
edited
Loading

Ok! Its important if it accelerates things we have an appreciation for what/how much 🙂

May I ask a naive question? Which distro of ros2 should I choose if I want to build the code and run some tests on the main branch nicely? Or should I just use the docker img that is mentioned in the documents?

@SteveMacenski
Copy link
Member

Rolling is the distribution of choice for main 😄

You are able to get Iron working as well, but involves a small handful of API changes

@SteveMacenski
Copy link
Member

@XinyuKhan what's the status here?

@SteveMacenski SteveMacenski linked an issue Apr 23, 2024 that may be closed by this pull request
[navfn_planner]Optimizations for the navfn planner in large map scenarios #4243
Open
@SteveMacenski
Copy link
Member

Note from author:

I currently found that navfn has the following problems:

  1. The performance loss caused by repeated allocation and release of memory that I mentioned in the issue

  2. calcPath function has bug when interpolate gradient

    According to the code at line 881 of navfn.cpp in the main branch, the value of dy dx should be limited between 0 and 1 to ensure the correctness of the interpolation method there. However, the code at line 911 only limits the range of dy and dx. Between -1 and 1, this will cause gradient interpolation exceptions when dx and dy take negative values. This problem occurs when setting the target point to the lower left relative to the current position of the mobile platform, and the planning results will jitter.

    The correct code in line 911 should be:
    while (dx > 1.0) {stc++; dx -= 1.0;}
    while (dx < 0.0) {stc--; dx += 1.0;}
    while (dy > 1.0) {stc += nx; dy -= 1.0;}

    while (dy < 0.0) {stc -= nx; dy += 1.0;}

  3. In line 550 of navfn.cpp, within the updateCellAstar method, there is a comparison between pot + dist and curT to form an "AStar" style wavefront. I think this is correct. However, I believe that when comparing l, r, u, d with pot + le, pot + re, pot + ue, pot + de, the pot should be the value before adding dist, not after. I am not entirely sure about this.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@SteveMacenski SteveMacenski SteveMacenski left review comments

At least 1 approving review is required to merge this pull request.

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

[navfn_planner]Optimizations for the navfn planner in large map scenarios
2 participants
@XinyuKhan @SteveMacenski